#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     IR1200,         sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     BL,            tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     FL,            tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     BR,            tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_2,     FR,            tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C3_1,     Arm,           tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     Lift,          tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C4_1,    Wrist,            tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"

void Forward (int power, int duration)						// Positive power means wheels going forward
{
	motor[FL] = power*1.5;
	motor[FR] = power;
	motor[BL] = power*1.5;
	motor[BR] = power;
	wait1Msec(duration);
}

void Backward (int power, int duration)
{
	motor[FL] = -1*power;
	motor[FR] = -1*power;
	motor[BL] = -1*power;
	motor[BR] = -1*power;
	wait1Msec(duration);
}

void turnL (int power, int duration)
{
	motor[FL] = -1*power;
	motor[FR] = power;
	motor[BL] = -1*power;
	motor[BR] = power;
	wait1Msec(duration);
}

void turnR (int power, int duration)
{
	motor[FL] = power;
	motor[FR] = -1*power;
	motor[BL] = power;
	motor[BR] = -1*power;
	wait1Msec(duration);
}

void initializeRobot ()
{
	motor[FL] = 0;
	motor[BR] = 0;
	motor[FR] = 0;
	motor[BL] = 0;
	motor[Arm] = 0;
	motor[Lift] = 0;
	wait1Msec(100);
}

task main()
{
	initializeRobot();

	waitForStart();

	servoTarget[Wrist] = 0;
	initializeRobot();
	Forward(50,1550);
	initializeRobot();
	motor[Lift] = 100;
	wait1Msec(600);
	initializeRobot();
	motor[Arm] = 40;
	wait1Msec(1700);
	initializeRobot();
	servoTarget[Wrist] = 120;
	initializeRobot();
	motor[Arm] = 30;
	wait1Msec(1000);
	initializeRobot();
	motor[Lift] = -100;
	wait1Msec(300);
	initializeRobot();
	servoTarget[Wrist] = 200;
	initializeRobot();
	motor[Lift] = -100;
	wait1Msec(300);
	initializeRobot();
	motor[Arm] = -30;
	wait1Msec(700);
	initializeRobot();
	Backward(30,700);
	initializeRobot();
}
